/*
 *  cpu_driver/interrupts_monitor.c
 *  
 *              CPU  Monitor 
 *     CPU INTERRUPT Monitor 
 *    Spring Semester.  044800.    Technion.
 *   Copyright (C) 2006  Evgeny Shulman  and Elizabeth Shtrom 
 */

#include <linux/kernel.h>
#include <linux/module.h>
#include <asm/system.h>
#include <asm/segment.h>
#include <asm/apic.h>

#include <asm/uaccess.h>
#include <linux/cpu.h>
#include <linux/config.h>
#include <linux/percpu.h>
#include <linux/spinlock.h>
#include <linux/delay.h>

#include "../cpu_consts.h"
#include "../cpu_monitor.h"
#include "../cpu_parse_display/cpu_display.h"
#include "interrupts_monitor.h"


#define HOOK_IDT_FROM 0
#define HOOK_IDT_TO IDT_ENTRIES
 
/*  XCHG DEFINITION   --> atomic replace in IDT TABLE */
static inline void cm_set_64bit (unsigned long long * ptr,
                unsigned int low, unsigned int high)
{
        __asm__ __volatile__ (
                "\n1:\t"
                "movl (%0), %%eax\n\t"
                "movl 4(%0), %%edx\n\t"
                "lock cmpxchg8b (%0)\n\t"
                /*"jnz 1b"*/
                : /* no outputs */
                :       "D"(ptr),
                        "b"(low),
                        "c"(high)
                :       "ax","dx","memory");
}


#define cm_ll_low(x)       *(((unsigned int*)&(x))+0)
#define cm_ll_high(x)      *(((unsigned int*)&(x))+1)

static inline void cm_set_64bit_var (unsigned long long *ptr,
                         unsigned long long value)
{
       cm_set_64bit(ptr,cm_ll_low(value), cm_ll_high(value));
}


/* end of XCHG definition */




// DEFINE_PER_CPU(struct im_irq_stat, mstat);
 struct im_irq_stat  mstat[MAXnrCPUs];
 #define mstat_cpu(cpu) mstat[cpu]
 #define mstat_this_cpu mstat[smp_processor_id() ]
 
 
// #define mstat_cpu(cpu)  per_cpu(mstat, cpu)
// #define mstat_this_cpu  __get_cpu_var(mstat)
// /* Must have preemption disabled for this to be meaningful. */
 
/**********************************************************************
 * 
 *  Mutexs to prevent hook of idt more than than one time
 *
 **********************************************************************/
int interrupts_monitor_hooked = 0;
int interrupts_monitor_handlers_hooked[IDT_ENTRIES] = {0};
spinlock_t hooked_mutex = SPIN_LOCK_UNLOCKED;
 
/* DEBUG VARIABLES*/
int i_was_wrong =0;
int i_last =0;


/********************************* HANDLERS ******************/



cm_gate_desc_t  idt_backup[IDT_ENTRIES];
void *  idt_backup_handlers[IDT_ENTRIES] = {NULL};
int _ds  = __USER_DS;
/**********************************************************************
 * 
 *   MACRO COUNTER_HANDLER
 *        define counter update function 
 **********************************************************************/
                    

#define COUNTER_HANDLER( number )                                       \
	asmlinkage void counter_handler_##number(void)                      \
	{                                                                   \
	 	mstat[smp_processor_id()].irqs[number] ++ ;                     \
		return ;	                                                    \
	}	



#define COUNTER_HANDLER_NO_SMP( number )                                       \
	asmlinkage void counter_handler_##number(void)                      \
	{                                                                   \
	 	mstat[0].irqs[number] ++ ;                     \
		return ;	                                                    \
	}	



#define COUNTER_HANDLER_ADDR( number ) \
	(void *)&counter_handler_##number	


#include "interrupts_monitor_counter.h"	


/**********************************************************************
 * 
 *  MACRO WRAPPER_HANDLER
 *    define new entry point for hooked interrupt  
 **********************************************************************/
#define WRAPPER_HANDLER( number )                                     \
 	extern void my_wrapper_##number(void);                            \
	void wrapper_handler_##number( void )                             \
	{                                                                 \
		__asm__ __volatile__ (                                        \
    	    ".globl my_wrapper_" #number "       \n"                  \
          "my_wrapper_" #number ":               \n"                  \
	      "	cld      \n" \
	      "pushl %%es;	 \n" \
	      "pushl %%ds    \n" \
          "pushl %%eax  \n" \
	      "pushl %%ebp  \n" \
	      "pushl %%edi  \n" \
	      "pushl %%esi  \n" \
	      "pushl %%edx  \n" \
	      "pushl %%ecx  \n" \
	      "pushl %%ebx  \n" \
	        "movl %2 , %%edx   \n" \
	      "movl %%edx, %%ds              \n" \
	      "movl %%edx, %%es              \n" \
	      "       call *%0         \n"     \
	      "	 popl %%ebx   \n" \
	      "  popl %%ecx   \n" \
	      "  popl %%edx   \n" \
	      "  popl %%esi   \n" \
	      "  popl %%edi   \n" \
	      "  popl %%ebp   \n" \
	      "  popl %%eax   \n" \
		  "	 popl %%ds    \n" \
          "	 popl %%es;	 \n" \
	      "       jmp  *%1                    \n"                     \
    	  "       iret                        \n"                     \
    	    ::"m"( my_counter_handlers[number] ),"m"( idt_backup_handlers[number]), "m"( _ds ) );\
  }                                                                   
  	                                                                           
#define WRAPPER_HANDLER_NO_SMP( number )                                     \
 	extern void my_wrapper_##number(void);                            \
	void wrapper_handler_##number( void )                             \
	{                                                                 \
		__asm__ __volatile__ (                                        \
    	    ".globl my_wrapper_" #number "       \n"                  \
          "my_wrapper_" #number ":               \n"                  \
	     "       call *%0         \n"     \
	      "       jmp  *%1                    \n"                     \
    	    ::"m"( my_counter_handlers[number] ),"m"( idt_backup_handlers[number]) );\
  }                                                                   
  	               


/**********************************************************************
 * 
 *  MACRO WRAPPER_HANDLER_ADDR
 *    returns my handler function adress
 **********************************************************************/
#define WRAPPER_HANDLER_ADDR( number ) \
	(void*) &my_wrapper_##number

#include "interrupts_monitor_wrapper.h"	



/**********************************************************************
 * 
 *  MACRO WRAPPER_HANDLER_ADDR
 *    returns handler address from descriptor
 **********************************************************************/
static void* get_handler(cm_gate_desc_t *idt_gate)
{
	return (void*)( (long ) (  (idt_gate->offset_h << 0x10) | idt_gate->offset) );
}

/**********************************************************************
 * 
 *  set_handler
 *
 *    replace adress of handler in provided interrupt descriptor.
 *
 **********************************************************************/
static void set_handler(cm_gate_desc_t *p_idt_gate, unsigned long new_adress)
{
	cm_gate_desc_t new_idt_gate;
	
	new_idt_gate = *p_idt_gate ;
 	new_idt_gate.offset    = (u16)( new_adress & 0x0000FFFF);
 	new_idt_gate.offset_h  = (u16)( new_adress >> 16);
//	if ( new_idt_gate.dpl ) {
//		PDEBUG("set_handler:changing dpl to 0 ! \n");
//		new_idt_gate.dpl = 0;
//	}
	PDEBUG("set_handler:from:%llx adress:0h %hx %hx \n",*(((unsigned long long *)p_idt_gate) ) , p_idt_gate->offset_h , p_idt_gate->offset);
	PDEBUG("set_handler:to  :%llx adress:0h %hx %hx \n",*((unsigned long long *)&(new_idt_gate) ) , new_idt_gate.offset_h,  new_idt_gate.offset );
/**********************/	
  cm_set_64bit_var( ( unsigned long long *)p_idt_gate ,*((unsigned long long *)&(new_idt_gate)) );
/*********************/	
	 
	PDEBUG("set_handler:now :%llx adress:0h %hx %hx \n",*(((unsigned long long *)p_idt_gate) ) , p_idt_gate->offset_h , p_idt_gate->offset);


}



/**********************************************************************
 * 
 *  start_interrupts_monitor
 *
 *    Starts the monitoring of the interrupts by replacing all interrupt
 *    handlers by our own (which forward requests to the old interrupts)
 *
 **********************************************************************/
int start_interrupts_monitor(void){
    
	int i;
	
	cm_gate_desc_t *idt,  *hooked_handler_idt = NULL;
  struct Xgt_desc_struct  idt_descr;
  
	void * my_handler;  
	PDEBUG("start_interrupts_monitor:checking hooked %d <-\n", interrupts_monitor_hooked);
	spin_lock( &hooked_mutex );
	if (interrupts_monitor_hooked) {
		spin_unlock( &hooked_mutex );
		return -1;
	}
	interrupts_monitor_hooked = 1;
	
	//:getting idtr.
	PDEBUG("start_interrupts_monitor:getting idtr<-\n");
	asm volatile("sidt %0" :: "m" (idt_descr));
	idt = (cm_gate_desc_t*) idt_descr.address;
	if( (long) idt < PAGE_OFFSET) return -2;  

	//copy idt to  idt_backup.
	memcpy((void *) &idt_backup, (void *)idt, IDT_ENTRIES * sizeof( cm_gate_desc_t ));
	PDEBUG("start_interrupts_monitor:back up idt :done\n");
	for(i =HOOK_IDT_FROM ; i < HOOK_IDT_TO; i++) {

#ifndef CPU_HOOK_PAGE_FAULT
	 	if  (   i == 14) 
			continue;
#endif 		

		if ((i==2) || (i==15) || ((i >= 20) && (i <= 31))) // Intel reserved
			continue;
		hooked_handler_idt = (void *)  (idt+i);
		
		
		//PDEBUG("start_interrupts_monitor:try hook handler 0x%x -->type 0x%x, present-> %d\n", i, hooked_handler_idt->type, hooked_handler_idt->present );
		if (        (hooked_handler_idt->type == CM_SYS_DESC_TYPE_TASKG)   // Task Gate - Not supported
			||    (hooked_handler_idt->present    == 0) ) // Not present
			continue;
			
			interrupts_monitor_handlers_hooked[i] = 1;  
				//Save the function address that is stored within the descriptor
			idt_backup_handlers[i] = get_handler(  idt_backup + i );
			my_handler = my_handlers[i];
						
			PDEBUG_WARN("hook handler 0x%x at adress %p --> %p \n", i, idt_backup_handlers[i],my_handler );
			local_irq_disable() ;
			set_handler ( idt+i ,  (unsigned long) my_handler); 
			local_irq_enable();
			
			PDEBUG("start_interrupts_monitor:hook handler 0x%x -->done, new handler is %p \n", i, get_handler(  idt + i ) );
	}
	
	spin_unlock(&hooked_mutex );
	return 0;
}


/**********************************************************************
 * 
 *  stop_interrupts_monitor
 *
 *    Stops the monitoring of the interrupts by retoring the previously
 *    saved IDT ( handlers).
 *
 **********************************************************************/
int stop_interrupts_monitor( void ){
	
 	int i;
	
	cm_gate_desc_t *idt,  *hooked_handler_idt = NULL;
  	struct Xgt_desc_struct  idt_descr;
 
 	
  	spin_lock( &hooked_mutex );
	if (!interrupts_monitor_hooked) {
		spin_unlock( &hooked_mutex );
		return -1;
	}
	interrupts_monitor_hooked = 0;
	

	asm volatile("sidt %0" :: "m" (idt_descr));
	idt = (cm_gate_desc_t *) idt_descr.address;
	for(i =HOOK_IDT_FROM ; i < HOOK_IDT_TO; i++) {
	
		if ((i==2) || (i==15) || ((i >= 20) && (i <= 31))) // Intel reserved
            continue;
	  	hooked_handler_idt = (void *)  (idt+i);
	  	if ( !interrupts_monitor_handlers_hooked[i]) 
	      continue;
		
		interrupts_monitor_handlers_hooked[i] = 0;
		PDEBUG_WARN("stop_interrupts_monitor:restore handler 0x%x  %p --> %p \n", i, hooked_handler_idt,get_handler(  idt_backup + i ) );
		
		local_irq_disable() ;
	 	cm_set_64bit_var( ( unsigned long long *)(idt+i) ,*((unsigned long long *)(idt_backup+i)) );
		local_irq_enable();
		PDEBUG("stop_interrupts_monitor:restore handler 0x%x -->done, new handler is %p \n", i, get_handler(  idt + i ) );
	}
	PDEBUG("stop_interrupts_monitor:Restoring handlers from backup is done\n");	
//	PDEBUG ( "-------------> i_was_wrong = %d, i_last = %d \n", i_was_wrong, i_last );
	
 	spin_unlock(&hooked_mutex );
	return 0;
}


/**********************************************************************
 * 
 * reset_interrupts_monitor
 *    reset interrupt monitor statistic struct for specific cpu.
 **********************************************************************/
int reset_interrupts_monitor(int cpu) {
	int i;
	for(i = 0; i < IDT_ENTRIES; i++) {
		mstat_cpu(cpu).irqs[i] = 0 ;
	}
	PDEBUG("reset_interrupts_monitor:reset counters %d<- done\n",cpu);
	return 0 ;
	
}
	

/**********************************************************************
 * 
 * reset_all_interrupts_monitor
 *    1.Stop interrupt monitor if it's running
 *    2.Reset interrupt monitor statistic struct for ALL CPUs
 *    3.Restart interrupt monitor if it was stopped.
 **********************************************************************/	
int reset_all_interrupts_monitor(void) {
	int i ,ret;
	int is_resume = 0;
	if ( is_interrupts_monitor() ){
		is_resume =1;
		ret = stop_interrupts_monitor();
		if( ret ) return ret;
	}
	
	PDEBUG("reset_all_interrupts_monitor:resume: %d\n\n\n",is_resume);
	for_each_online_cpu(i) {
		PDEBUG("cpu_monitor_init:reset counters for %d cpu\n", i);
		reset_interrupts_monitor(i);
	}
	if ( is_resume) {
		ret = start_interrupts_monitor();
		if( ret ) return ret;
	}
	return 0 ;
}


/**********************************************************************
 * 
 * is_interrupts_monitor
 *   return if interrupt monitor is running currently
 **********************************************************************/
int is_interrupts_monitor( void ){
	int i;
	spin_lock( &hooked_mutex );
	i = interrupts_monitor_hooked;
	spin_unlock( &hooked_mutex );
	return i;
}

/**********************************************************************
 * 
 * read_interrupts_monitor
 *   read stats for cpu it's currently running on from interrupt monitor struct. 
 **********************************************************************/
void read_interrupts_monitor (void *v){
	int i;
	im_irq_stat_s *irq_stat =  (  im_irq_stat_s *) v; 
	
	for(i = 0; i < IDT_ENTRIES; i++) {
		irq_stat->irqs[i] = mstat_cpu( smp_processor_id() ).irqs[i];
	}
	return ;
}
 
 
/**********************************************************************
 * MORE SIMPLE IMPLEMENTATION OF CPU MONITOR --> kernel recompilation required!
 **********************************************************************/
 
//irqreturn_t interrupts_monitor_hooked_handler ( int irq , void *dev_id , struct 	pt_regs *regs ){
//	 mstat_this_cpu.irqs[irq]++;
//	/* here we call old handler!*/ 
//	 return   ( *interrupts_monitor_handler[irq] )(irq , dev_id , regs );
//}
//
//

//
//int start_interrupts_monitor(void)
//{
//	
//	int i, j ;
//	irq_desc_t *desc;
//	struct irqaction *action;
//
//	if (interrupts_monitor_hooked) return 0;
//	
//	
//	
//	for_each_online_cpu(j)
//			reset_interrupts_monitor(j);
//	
//	for(i = 1; i < IDT_ENTRIES; i++) {
//		if ((i==2) || (i==15) || ((i >= 20) && (i <= 31))) // Intel reserved
//            continue;
//    if(  interrupts_monitor_handler_is_hooked[i] )
//    	 continue;
//  	
//  	desc = irq_descp(i);
//		spin_lock(&desc->lock);
//		
//		action = desc->action;
//		if (!action)          
//		{
//			spin_unlock(&desc->lock);
//			continue;
//		}
//		
//		interrupts_monitor_handler[i] = action->handler;
//		action->handler =  interrupts_monitor_hooked_handler;
//		interrupts_monitor_handler_is_hooked[i] = 1;			
//		
//		spin_unlock(&desc->lock);
//	}
//	
//	interrupts_monitor_hooked = 1;
//	return 0;
//}
//
//int stop_interrupts_monitor(void)
//{
//	
//	int i;
//	irq_desc_t *desc;
//	
//	if (!interrupts_monitor_hooked) return 0;
//	
//	for(i = 1; i < IDT_ENTRIES; i++) {
//		struct irqaction *action;
//
//		desc = irq_descp(i);
//		spin_lock(&desc->lock);
//		action = desc->action;
//		action->handler = interrupts_monitor_handler[i];
//		spin_unlock(&desc->lock);
//	
//	}
//	
//	interrupts_monitor_hooked = 0;
//	return 0;
//}
